
#include <gl/freeglut.h>
//#include <gl/glut.h>
#include <stdio.h>
#include "SceneNode.h"
#include "Vector3d.h"
#include "Quaternion.h"


/* DECLARATIONS */
/*---------------------------------------------------------------------------------*/
void reshape( int width, int height );
void RenderCallBack(void);
void quatToMatrix( Quaternion* q );
void prepSceneTree();
int prepMenus( SceneNode* r );
void sceneNodeSelectionMenu();
void axisSelectionMenu( int n );
void mainMenu( int n );
void keyboardCallback( int key, int x, int y );
SceneNode* findById( SceneNode* r, int id );
void paintSceneTree( SceneNode* root );
/*---------------------------------------------------------------------------------*/

/* GLOBALS */
/*---------------------------------------------------------------------------------*/
GLdouble m[16];
SceneNode* root;
SceneNode* selected_node;
Vector3d rotation_axis;
/*---------------------------------------------------------------------------------*/

/*  MAIN  */
/*---------------------------------------------------------------------------------*/
int main( int argc, char** argv )
{
	int menu1, menu2;
	glutInitWindowPosition(20,20);

	// define position of window in the screen
	glutInitWindowSize(500,500);

	// define the window size
	glutInit( &argc, argv );

	// initialize OpenGL
	glutInitDisplayMode(GLUT_RGBA);

	// define display mode
	glutCreateWindow("hw1");
	glutDisplayFunc(RenderCallBack);
	glutReshapeFunc(reshape);
	glutSpecialFunc(keyboardCallback);

	prepSceneTree();
	menu1 = glutCreateMenu(axisSelectionMenu);
	glutAddMenuEntry( "x axis", -1 );
	glutAddMenuEntry( "y axis", -2 );
	glutAddMenuEntry( "z axis", -3 );
	menu2 = prepMenus( root );
	glutCreateMenu(mainMenu);
	glutAddSubMenu( "select axis",menu1 );
	glutAddSubMenu( "select scene node",menu2 );
	glutAttachMenu( GLUT_RIGHT_BUTTON );

	glutMainLoop();
	SceneNode_destroy(root);
	return 0;
}
/*---------------------------------------------------------------------------------*/


/* glut plugs */
/*---------------------------------------------------------------------------------*/
void mainMenu( int n ){}

void keyboardCallback( int key, int x, int y )
{
	Quaternion q;

	if( key==GLUT_KEY_LEFT || key==GLUT_KEY_RIGHT )
	{
		q = (key==GLUT_KEY_LEFT) ? Quaternion_fromAxisAngle( &rotation_axis,  3.14159265/45 )
								 : Quaternion_fromAxisAngle( &rotation_axis, -3.14159265/45 );
		selected_node->orientation = Quaternion_multiply( &selected_node->orientation, &q );
		selected_node->orientation = Quaternion_normalized( &selected_node->orientation );
	}
}

void axisSelectionMenu( int n )
{
	switch(n)
	{
	case -1: rotation_axis = Vector3d_FromValues(1,0,0); break;
	case -2: rotation_axis = Vector3d_FromValues(0,1,0); break;
	case -3: rotation_axis = Vector3d_FromValues(0,0,1); break;
	}
}

void sceneNodeSelectionMenu( int n )
{
	selected_node = findById(root, n);
}

void RenderCallBack(void)
{
	GLdouble mat [4][4]={0};

	glClearColor(0.5f,0.5f,0.5f,1.0f);
	glClear( GL_COLOR_BUFFER_BIT );

	glMatrixMode( GL_MODELVIEW );
	glPushMatrix();
	//gluLookAt( 10, 10, 10, 0, 0, 0, 0, 1, 0 );
	glColor3d(1,1,1);
	glTranslated(0,0,-5);
	paintSceneTree(root);
	glPopMatrix();
	glutSwapBuffers();
	glutPostRedisplay();
}

void reshape( int width, int height )
{
	GLdouble aspect_ratio, left, right, top, bottom;

	left	=-20;
	right	= 20;
	top		= 20;
	bottom	=-20;

	glMatrixMode( GL_PROJECTION );
	glLoadIdentity();

	glViewport( 0, 0, width, height );

	aspect_ratio = width / (GLdouble)height;

	if( aspect_ratio>=1 )
	{
		left	*= aspect_ratio;
		right	*= aspect_ratio;
	}

	else
	{
		top		/= aspect_ratio;
		bottom	/= aspect_ratio;
	}

	glOrtho( left, right, bottom, top, 0, 200 );
	//glFrustum( left, right, bottom, top, 10, 100 );
}
/*---------------------------------------------------------------------------------*/

/* helpers */
/*---------------------------------------------------------------------------------*/
void quatToMatrix( Quaternion* q )
{
	m[0]  = 1 - 2*(q->y*q->y + q->z*q->z);
	m[1]  =		2*(q->x*q->y - q->z*q->w);
	m[2]  =		2*(q->x*q->z + q->y*q->w);
	m[3]  =		0;

	m[4]  =		2*(q->x*q->y + q->z*q->w);
	m[5]  = 1 -	2*(q->x*q->x + q->z*q->z);
	m[6]  =		2*(q->y*q->z - q->x*q->w);
	m[7]  =		0;

	m[8]  =		2*(q->x*q->z - q->y*q->w);
	m[9]  =		2*(q->y*q->z + q->x*q->w);
	m[10] = 1 - 2*(q->x*q->x + q->y*q->y);
	m[11] =		0;

	m[12] =		0;
	m[13] =		0;
	m[14] =		0;
	m[15] =		1;

	glMultMatrixd(m);
}

int prepMenus( SceneNode* r )
{
	int menu,submenu, i;
	
	menu = glutCreateMenu(sceneNodeSelectionMenu);
	glutAddMenuEntry(r->name, r->id);
	
	for( i=0 ; i<r->child_count ; ++i )
	{
		if( r->children[i]->child_count==0 )
			glutAddMenuEntry(r->children[i]->name, r->children[i]->id);
		else
		{
			submenu = prepMenus( r->children[i] );
			glutSetMenu( menu );
			glutAddSubMenu( r->children[i]->name, submenu );
		}
	}
	return menu;
}

void prepSceneTree()
{
	Vector3d v;
	Quaternion q;

	v = Vector3d_FromValues(0,0,0);
	q = Quaternion_fromValues(1,0,0,0);

	root = SceneNode_FromValues( "root", 4, &v, &q );
	selected_node = root;

	v = Vector3d_FromValues(0,3,0);
	root->children[0] = SceneNode_FromValues( "head", 0, &v, &q );

	v = Vector3d_FromValues(3,0,0);
	root->children[1] = SceneNode_FromValues( "left_shoulder", 1, &v, &q );
	root->children[1]
		->children[0]= SceneNode_FromValues( "left_elbow", 1, &v, &q );
	root->children[1]
		->children[0]
		->children[0]= SceneNode_FromValues( "left_wrist", 0, &v, &q );

	v = Vector3d_FromValues(-3,0,0);
	root->children[2] = SceneNode_FromValues( "right_shoulder", 1, &v, &q );
	root->children[2]
		->children[0]= SceneNode_FromValues( "right_elbow", 1, &v, &q );
	root->children[2]
		->children[0]
		->children[0]= SceneNode_FromValues( "right_wrist", 0, &v, &q );

	v = Vector3d_FromValues(0,-5,0);
	root->children[3] = SceneNode_FromValues( "pelvis", 2, &v, &q );

	v = Vector3d_FromValues(2,-2,0);
	root->children[3]
		->children[0] = SceneNode_FromValues( "left_hip", 1, &v, &q );

	v = Vector3d_FromValues(0,-5,0);
	root->children[3]
		->children[0]
		->children[0]= SceneNode_FromValues( "right_knee", 1, &v, &q );
	root->children[3]
		->children[0]
		->children[0]
		->children[0]= SceneNode_FromValues( "right_foot", 0, &v, &q );

	v = Vector3d_FromValues(-2,-2,0);
	root->children[3]
	->children[1] = SceneNode_FromValues( "right_hip", 1, &v, &q );

	v = Vector3d_FromValues(0,-5,0);
	root->children[3]
		->children[1]
		->children[0]= SceneNode_FromValues( "right_knee", 1, &v, &q );
	root->children[3]
		->children[1]
		->children[0]
		->children[0]= SceneNode_FromValues( "right_foot", 0, &v, &q );
};

void paintSceneTree( SceneNode* root )
{
	int i;
	glPushMatrix();
	glTranslated( (GLdouble)root->position.x, 
				  (GLdouble)root->position.y, 
				  (GLdouble)root->position.z );
	quatToMatrix( &root->orientation );
	glutWireCube(1);

	for( i=0 ; i<root->child_count ; ++i )
	{
		paintSceneTree( root->children[i] );
	}
	glPopMatrix();
};

SceneNode* findById( SceneNode* r, int id )
{
	int i;
	SceneNode* s=NULL;

	if( r->id==id )
		return r;

	for( i=0 ; i<r->child_count ; ++i )
	{
		s = findById(r->children[i], id);
		if(s!=NULL)
			return s;
	}

	return NULL;
};
/*---------------------------------------------------------------------------------*/
